#pragma once

#include "types.h"
#include <type_traits>

OPEN_JLIB_NS


template<typename Type = uwl>
struct Bits {

  static_assert(std::is_integral<Type>::value, "");


  /**
   * Highest one bit
   */
  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 1, int> = 0>
  static T highest (T v) noexcept {
#if __x86_64__
    if (!v) return 0;
    u32 d;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"((u32)(U)v): "cc");
    return (T)1 << d;
#elif __aarch64__
    u32 d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"((u32)v));
    return (U)0x80 >> d;
#else
    U u = (U)v;
    u |= u >> 1;
    u |= u >> 2;
    u |= u >> 4;
    return (T)(u - (u >> 1));
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 2, int> = 0>
  static T highest (T v) noexcept {
#if __x86_64__
    if (!v) return 0;
    T d;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return (T)1 << d;
#elif __aarch64__
    u32 d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"((u32)v));
    return (U)0x8000 >> d;
#else
    U u = (U)v;
    u |= u >> 1;
    u |= u >> 2;
    u |= u >> 4;
    u |= u >> 8;
    return (T)(u - (u >> 1));
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 4, int> = 0>
  static T highest (T v) noexcept {
#if __x86_64__
    if (!v) return 0;
    T d;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return (T)1 << d;
#elif __aarch64__
    T d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"(v));
    return (U)0x80000000 >> d;
#else
    U u = (U)v;
    u |= u >>  1;
    u |= u >>  2;
    u |= u >>  4;
    u |= u >>  8;
    u |= u >> 16;
    return (T)(u - (u >> 1));
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 8, int> = 0>
  static T highest (T v) noexcept {
#if __x86_64__
    if (!v) return 0;
    T d;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return (T)1 << d;
#elif __aarch64__
    T d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"(v));
    return (U)0x8000000000000000L >> d;
#else
    U u = (U)v;
    u |= u >>  1;
    u |= u >>  2;
    u |= u >>  4;
    u |= u >>  8;
    u |= u >> 16;
    u |= u >> 32;
    return (T)(u - (u >> 1));
#endif
  }


  /**
   * Lowest one bit
   */
  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 1, int> = 0>
  static T lowest (T v) noexcept {
    return -v & v;
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 2, int> = 0>
  static T lowest (T v) noexcept {
    return -v & v;
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 4, int> = 0>
  static T lowest (T v) noexcept {
#if __x86_64__
    T d;
    asm volatile ("BLSI %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return d;
#else
    return -v & v;
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 8, int> = 0>
  static T lowest (T v) noexcept {
#if __x86_64__
    T d;
    asm volatile ("BLSI %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return d;
#else
    return -v & v;
#endif
  }


  /**
   * Count leading zero bit
   */
  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 1, int> = 0>
  static T leading0 (T v) noexcept {
#if __x86_64__
    if (!v) return 8;
    u32 d, t = (U)v;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"(t): "cc");
    return (T)(7 - d);
#elif __aarch64__
    u32 d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"((u32)v));
    return (T)(d - 24);
#else
    if (!v) return 8;
    U u = (U)v, n = 1;
    if (u >> 4 == 0) { n += 4; u <<= 4; }
    if (u >> 6 == 0) { n += 2; u <<= 2; }
    return n - (u >> 7);
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 2, int> = 0>
  static T leading0 (T v) noexcept {
#if __x86_64__
    if (!v) return 16;
    T d;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return 15 - d;
#elif __aarch64__
    u32 d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"((u32)v));
    return (T)(d - 16);
#else
    if (!v) return 16;
    U u = (U)v, n = 1;
    if (u >>  8 == 0) { n += 8; u <<= 8; }
    if (u >> 12 == 0) { n += 4; u <<= 4; }
    if (u >> 14 == 0) { n += 2; u <<= 2; }
    return n - (u >> 15);
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 4, int> = 0>
  static T leading0 (T v) noexcept {
#if __x86_64__
    if (!v) return 32;
    T d;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return 31 - d;
#elif __aarch64__
    T d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    if (!v) return 32;
    U u = (U)v, n = 1;
    if (u >> 16 == 0) { n += 16; u <<= 16; }
    if (u >> 24 == 0) { n +=  8; u <<=  8; }
    if (u >> 28 == 0) { n +=  4; u <<=  4; }
    if (u >> 30 == 0) { n +=  2; u <<=  2; }
    return n - (u >> 31);
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 8, int> = 0>
  static T leading0 (T v) noexcept {
#if __x86_64__
    if (!v) return 64;
    T d;
    asm volatile ("BSR %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return 63 - d;
#elif __aarch64__
    T d;
    asm volatile ("CLZ %[dst], %[src]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    if (!v) return 64;
    U u = (U)v, n = 1;
    if (u >> 32 == 0) { n += 32; u <<= 32; }
    if (u >> 48 == 0) { n += 16; u <<= 16; }
    if (u >> 56 == 0) { n +=  8; u <<=  8; }
    if (u >> 60 == 0) { n +=  4; u <<=  4; }
    if (u >> 62 == 0) { n +=  2; u <<=  2; }
    return n - (u >> 63);
#endif
  }


  /**
   * Count trailing zero
   */
  template<typename T = Type, typename U = u8, std::enable_if_t<sizeof(T) == 1, int> = 0>
  static T trailing0 (T v) noexcept {
#if __x86_64__
    if (!v) return 8;
    u32 d;
    asm volatile ("BSF %[src], %[dst]": [dst]"=r"(d): [src]"r"((u32)(U)v): "cc");
    return (T)d;
#elif __aarch64__
    u32 d;
    asm volatile ("RBIT %[dst], %[src]\n\tCLZ %[dst], %[dst]": [dst]"=r"(d): [src]"r"((u32)v << 24));
    return (T)(d - 24);
#else
    if (!v) return 8;
    U u = (U)v, t, n = 7;
    t = u <<  4; if (t) { n -= 4; u = t; }
    t = u <<  2; if (t) { n -= 2; u = t; }
    return n - ((u << 1) >> 7);
#endif
  }

  template<typename T = Type, typename U = u8, std::enable_if_t<sizeof(T) == 2, int> = 0>
  static T trailing0 (T v) noexcept {
#if __x86_64__
    if (!v) return 16;
    T d;
    asm volatile ("BSF %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return d;
#elif __aarch64__
    u32 d;
    asm volatile ("RBIT %[dst], %[src]\n\tCLZ %[dst], %[dst]": [dst]"=r"(d): [src]"r"((u32)v << 16));
    return (T)(d - 16);
#else
    if (!v) return 16;
    U u = (U)v, t, n = 15;
    t = u <<  8; if (t) { n -= 8; u = t; }
    t = u <<  4; if (t) { n -= 4; u = t; }
    t = u <<  2; if (t) { n -= 2; u = t; }
    return n - ((u << 1) >> 15);
#endif
  }

  template<typename T = Type, typename U = u8, std::enable_if_t<sizeof(T) == 4, int> = 0>
  static T trailing0 (T v) noexcept {
#if __x86_64__
    if (!v) return 32;
    T d;
    asm volatile ("BSF %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return d;
#elif __aarch64__
    T d;
    asm volatile ("RBIT %[dst], %[src]\n\tCLZ %[dst], %[dst]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    if (!v) return 32;
    U u = (U)v, t, n = 31;
    t = u << 16; if (t) { n -= 16; u = t; }
    t = u <<  8; if (t) { n -=  8; u = t; }
    t = u <<  4; if (t) { n -=  4; u = t; }
    t = u <<  2; if (t) { n -=  2; u = t; }
    return n - ((u << 1) >> 31);
#endif
  }

  template<typename T = Type, typename U = u8, std::enable_if_t<sizeof(T) == 8, int> = 0>
  static T trailing0 (T v) noexcept {
#if __x86_64__
    if (!v) return 64;
    T d;
    asm volatile ("BSF %[src], %[dst]": [dst]"=r"(d): [src]"r"(v): "cc");
    return d;
#elif __aarch64__
    T d;
    asm volatile ("RBIT %[dst], %[src]\n\tCLZ %[dst], %[dst]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    if (!v) return 64;
    U u = (U)v, t, n = 63;
    t = u << 32; if (t) { n -= 32; u = t; }
    t = u << 16; if (t) { n -= 16; u = t; }
    t = u <<  8; if (t) { n -=  8; u = t; }
    t = u <<  4; if (t) { n -=  4; u = t; }
    t = u <<  2; if (t) { n -=  2; u = t; }
    return n - ((u << 1) >> 31);
#endif
  }

  /**
   * Count one bit
   */
  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 1, int> = 0>
  static T count (T v) noexcept {
#if __x86_64__
    u32 d;
    asm volatile ("POPCNT %[src], %[dst]": [dst]"=r"(d): [src]"r"((u32)(U)v));
    return (T)d;
#else
    U u = (U)v;
    u = u - ((u >> 1) & (U)0x55);
    u = (u & (U)0x33) + ((u >> 2) & (U)0x33);
    u = (u + (u >> 4)) & (U)0x0F;
    return (T)u;
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 2, int> = 0>
  static T count (T v) noexcept {
#if __x86_64__
    T d;
    asm volatile ("POPCNT %[src], %[dst]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    U u = (U)v;
    u = u - ((u >> 1) & (U)0x5555);
    u = (u & (U)0x3333) + ((u >> 2) & (U)0x3333);
    u = (u + (u >> 4)) & (U)0x0F0F;
    u = u + (u >> 8);
    u = u & (U)0x1F;
    return (T)u;
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 4, int> = 0>
  static T count (T v) noexcept {
#if __x86_64__
    T d;
    asm volatile ("POPCNT %[src], %[dst]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    U u = (U)v;
    u = u - ((u >> 1) & (U)0x55555555);
    u = (u & (U)0x33333333) + ((u >> 2) & (U)0x33333333);
    u = (u + (u >> 4)) & (U)0x0F0F0F0F;
    u = u + (u >> 8);
    u = u + (u >> 16);
    u = u & (U)0x3F;
    return (T)u;
#endif
  }
  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 8, int> = 0>
  static T count (T v) noexcept {
#if __x86_64__
    T d;
    asm volatile ("POPCNT %[src], %[dst]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    U u = (U)v;
    u = u - ((u >> 1) & (U)0x5555555555555555UL);
    u = (u & (U)0x3333333333333333UL) + ((u >> 2) & (U)0x3333333333333333UL);
    u = (u + (u >> 4)) & (U)0x0F0F0F0F0F0F0F0FUL;
    u = (u * (U)0x0101010101010101UL) >> 56;
    return (T)u;
#endif
  }


  static Type test (Type v, Type m) noexcept {
    return v & m;
  }

  static Type testz (Type v, Type m) noexcept {
#if defined(__x86_64__)
    return x86_andn(v, m);
#else
    return ~v & m;
#endif
  }


  static void set (Type& v, Type m) noexcept {
    v |= m;
  }

  static void reset (Type& v, Type m) noexcept {
#if __x86_64__
    asm volatile ("ANDN %[dst], %[msk], %[dst]": [dst]"+r"(v): [msk]"r"(m));
#else
    v &= ~m;
#endif
  }

  static void complement (Type& v, Type m) noexcept {
    v ^= m;
  }


  static Type getAndSet (Type& v, Type m) noexcept {
    auto x = v & m;
    set(v, m);
    return x;
  }

  static Type getAndReset (Type& v, Type m) noexcept {
    auto x = v & m;
    reset(v, m);
    return x;
  }

  static Type getAndComplement (Type& v, Type m) noexcept {
    auto x = v & m;
    complement(v, m);
    return x;
  }


  static Type setAndChanged (Type& v, Type m) noexcept {
    auto x = v;
    set(v, m);
    return x ^ m;
  }

  static Type resetAndChanged (Type& v, Type m) noexcept {
    auto x = v;
    reset(v, m);
    return x ^ m;
  }


  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 1, int> = 0>
  static T reverse (T v) noexcept {
#if __aarch64__
    u32 d;
    asm volatile ("RBIT %[dst], %[src]": [dst]"=r"(d): [src]"r"((u32)(U)v));
    return (T)(d >> 24);
#else
    U u = (U)v;
    u = (u << 4) | (u >> 4);
    u = ((u >> 2) & (U)0x33) | ((u & (U)0x33) << 2);
    u = ((u >> 1) & (U)0x55) | ((u & (U)0x55) << 1);
    return (T)u;
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 2, int> = 0>
  static T reverse (T v) noexcept {
#if __aarch64__
    u32 d;
    asm volatile ("RBIT %[dst], %[src]": [dst]"=r"(d): [src]"r"((u32)(U)v));
    return (T)(d >> 16);
#else
    U u = (U)v;
    u = (u << 8) | (u >> 8);
    u = ((u >> 4) & (U)0x0F0F) | ((u & (U)0x0F0F) << 4);
    u = ((u >> 2) & (U)0x3333) | ((u & (U)0x3333) << 2);
    u = ((u >> 1) & (U)0x5555) | ((u & (U)0x5555) << 1);
    return (T)u;
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 4, int> = 0>
  static T reverse (T v) noexcept {
#if __aarch64__
    T d;
    asm volatile ("RBIT %[dst], %[src]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    U u = (U)v;
#if __x86_64__
    asm volatile ("BSWAP %[reg]": [reg]"+r"(u));
#else
    u = (u << 16) | (u >> 16);
    u = ((u >> 8) & (U)0x00FF00FF) | ((u & (U)0x00FF00FF) << 8);
#endif
    u = ((u >> 4) & (U)0x0F0F0F0F) | ((u & (U)0x0F0F0F0F) << 4);
    u = ((u >> 2) & (U)0x33333333) | ((u & (U)0x33333333) << 2);
    u = ((u >> 1) & (U)0x55555555) | ((u & (U)0x55555555) << 1);
    return (T)u;
#endif
  }

  template<typename T = Type, typename U = std::make_unsigned_t<T>, std::enable_if_t<sizeof(T) == 8, int> = 0>
  static T reverse (T v) noexcept {
#if __aarch64__
    T d;
    asm volatile ("RBIT %[dst], %[src]": [dst]"=r"(d): [src]"r"(v));
    return d;
#else
    U u = (U)v;
#if __x86_64__
    asm volatile ("BSWAP %[reg]": [reg]"+r"(u));
#else
    u = (u << 32) | (u >> 32);
    u = ((u >> 16& (U)0x0000FFFF0000FFFFUL) | ((u & (U)0x0000FFFF0000FFFFUL) << 16);
    u = ((u >>  8& (U)0x00FF00FF00FF00FFUL) | ((u & (U)0x00FF00FF00FF00FFUL) <<  8);
#endif
    u = ((u >> 4) & (U)0x0F0F0F0F0F0F0F0FUL) | ((u & (U)0x0F0F0F0F0F0F0F0FUL) <<  4);
    u = ((u >> 2) & (U)0x3333333333333333UL) | ((u & (U)0x3333333333333333UL) <<  2);
    u = ((u >> 1) & (U)0x5555555555555555UL) | ((u & (U)0x5555555555555555UL) <<  1);
    return (T)u;
#endif
  }
};


CLOSE_JLIB_NS

